#include "joyops.h"

int xyspace (CSOUND *csound, XYSPACE *space)
{
  int sign;
  MYFLT absv;
  if(UNLIKELY(*space->kcurve != space->pcurve)) {
    if(UNLIKELY(*space->kcurve > 0.0 &&
		(void *)(space->ftp = csound->FTFind(csound, space->kcurve))
		== NULL)) {
      csound->Warning(csound, Str("xyspace: No such table %f"),
		      space->kcurve);
      return OK;
    }
    space->pcurve = *space->kcurve;
  }
  /*
  if(space->pcurve != 0) {
    *space->kdx = min(abs(*space->kdx), 1) * signum(*space->kdx);
    space->posx +=
      space->ftp->ftable[ abs(*space->kdx) * space->ftp->flen ] *
      signum(*space->kdx);
    *space->kdy = min(abs(*space->kdy), 1) * signum(*space->kdy);
    space->posy +=
      space->ftp->ftable[ abs(*space->kdy) * space->ftp->flen ] *
      signum(*space->kdy);
      } else { */
  space->posx += *space->kdx;
  space->posy += *space->kdy;
  //}
  if (*space->klimtype >= 1.0) {
    if(*space->klimtype < 2.0) {
      /* clip position at +/- klimit */
      sign = space->posx < 0 ? -1 : 1;
      absv = abs(space->posx);
      *space->kposx = absv < *space->klimit ?
	space->posx : *space->klimit * sign;
      sign = space->posy < 0 ? -1 : 1;
      absv = abs(space->posy);
      *space->kposy = absv < *space->klimit ?
	space->posy : *space->klimit * sign;
    } else {
      /* wrap position at +/- klimit */

      space->posx = fmod(space->posx+*space->klimit, *space->klimit*2);
      *space->kposx = space->posx - *space->klimit;
      space->posy = fmod(space->posy+*space->klimit, *space->klimit*2);
      *space->kposy = space->posy - *space->klimit;
    }
  } else {
    /* no range control */
    *space->kposx = space->posx;
    *space->kposy = space->posy;
  }
  return OK;
}

int cart2polar (CSOUND *csound, CART2POLAR *cart) {
  *cart->kr = sqrt(*cart->x * *cart->x + *cart->y * *cart->y);
  *cart->ktheta = atan2(*cart->x, *cart->y);
  return OK;
}

int angle (CSOUND *csound, ANGLE *ang) {
  MYFLT dx, dy;
  dx = *ang->x1 - ang->x0;
  ang->x0 = *ang->x1;
  dy = *ang->y1 - ang->y0;
  ang->y0 = *ang->y1;
  *ang->kr = sqrt(dx * dx + dy * dy);
  *ang->ktheta = atan2(dx, dy);
  return OK;
}

int rotation (CSOUND *csound, ROTATION *rot) {
  MYFLT r, theta;
  r = sqrt(*rot->x * *rot->x + *rot->y * *rot->y);
  theta = atan2(*rot->x, *rot->y);

  *rot->kdeltaR = r - rot->prevR;
  *rot->kdeltaTheta = theta - rot->prevTheta;

  rot->prevTheta = theta;
  rot->prevR = r;

  if (*rot->kdeltaTheta > M_PI) {
    *rot->kdeltaTheta -= 2*M_PI;
  } else if (*rot->kdeltaTheta < -M_PI) {
    *rot->kdeltaTheta += 2*M_PI;
  }

  return OK;
}

static OENTRY localops[] = {
  { "xyspace", sizeof(XYSPACE), 2, "kk", "kkkkk", NULL, (SUBR) xyspace, NULL },
  { "cart2polar", sizeof(CART2POLAR), 2, "kk", "kk", NULL, (SUBR) cart2polar,
    NULL },
  { "angle", sizeof(ANGLE), 2, "kk", "kk", NULL, (SUBR) angle, NULL },
  { "rotation", sizeof(ROTATION), 2, "kk", "kk", NULL, (SUBR) rotation, NULL },
};

LINKAGE
